#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/laser_scan.hpp>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/registration/icp.h>
#include <pcl_conversions/pcl_conversions.h>
#include <tf2_ros/static_transform_broadcaster.h>
#include <tf2_ros/transform_broadcaster.h>
#include <geometry_msgs/msg/transform_stamped.hpp>

#include "fuction_temp.hpp"

//using namespace std;
using namespace std::chrono_literals;

class LaserToTF : public rclcpp::Node {
public:
    LaserToTF() : Node("laser_to_tf") {
        this->declare_parameter<int>("yaml_run_mode",0);
        this->declare_parameter<float>("seat_x0",0);
        this->declare_parameter<float>("seat_y0",0);
        yaml_run_mode =this->get_parameter("yaml_run_mode").as_int();
        seat_x0 = this->get_parameter("seat_x0").as_double();
        seat_y0 = this->get_parameter("seat_y0").as_double();

        RCLCPP_INFO(this->get_logger(),"this node run mode=%d seat= %5.2f %5.2f",yaml_run_mode,seat_x0,seat_y0);
        // 订阅 cloud 消息
        cloud_sub = this->create_subscription<sensor_msgs::msg::PointCloud2>("/cloud", 
            10, std::bind(&LaserToTF::cloud_topic_callback, this, std::placeholders::_1));
        // 订阅 LaserScan 消息
        // laser_sub_ = this->create_subscription<sensor_msgs::msg::LaserScan>(
        //     "scan", 10, std::bind(&LaserToTF::laserCallback, this, std::placeholders::_1));

        // 创建 TF 广播器
        tf_broadcaster_ = std::make_shared<tf2_ros::TransformBroadcaster>(this);

        // 初始化标志位
        first_frame_received_ = false;
        count=0;
    }

private:
void cloud_topic_callback(const sensor_msgs::msg::PointCloud2::SharedPtr msg)
 {
    pcl::PointCloud<pcl::PointXYZ> laser_cloud;
    pcl::fromROSMsg(*msg, laser_cloud);
    RCLCPP_INFO(this->get_logger(), "Received=%d",++count);
    if (laser_cloud.empty()) {
        RCLCPP_WARN(this->get_logger(), "Received empty point cloud. Skipping ICP.");
        return;
    }

    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_ptr(new pcl::PointCloud<pcl::PointXYZ>);
    size_t sz = laser_cloud.size();
     
    for(int i=0;i<sz;++i) cloud_ptr->points.push_back(laser_cloud.points[i]);
    // 设置目标点云的高度和宽度
        cloud_ptr->width = static_cast<int>(laser_cloud.points.size());
        cloud_ptr->height = 1;
    //*cloud_ptr = laser_cloud;
   
    if (!first_frame_received_) {
        // 第一帧作为参考点云
       
        init_cloud_ = cloud_ptr;
        first_frame_received_ = true;
        RCLCPP_INFO(this->get_logger(), "Received first frame as reference. Points: %d", init_cloud_->points.size());
        
       // pcl::io::savePCDFile("saveName.pcd", *init_cloud_);
    } else {

       RCLCPP_INFO(this->get_logger(), "Rec");

       
    }
    
}
    void laserCallback(const sensor_msgs::msg::LaserScan::SharedPtr msg) {
        // 将 LaserScan 转换为点云
        pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_temp(new pcl::PointCloud<pcl::PointXYZ>);
        pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
        convertLaserToPointCloud(msg, cloud_temp);
        move_st_point(cloud_temp,cloud);
        if (!first_frame_received_) {
            // 第一帧作为参考点云
            reference_cloud_ = cloud;
            first_frame_received_ = true;
            RCLCPP_INFO(this->get_logger(), "Received first frame as reference.");
        } else {
            // 使用 ICP 配准当前帧和参考帧
            pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
              
            icp.setInputSource(cloud);
            icp.setInputTarget(reference_cloud_);
          
               // 设置最大对应距离
        double max_correspondence_distance = 0.05;  // 根据实际情况调整
        icp.setMaxCorrespondenceDistance(max_correspondence_distance);
           
      
        icp.setMaximumIterations(100);  // 最大迭代次数
        icp.setTransformationEpsilon(1e-6);  // 变换阈值
        icp.setEuclideanFitnessEpsilon(0.001);  // 欧氏距离适配度阈值
        
            pcl::PointCloud<pcl::PointXYZ> aligned_cloud;
      
            icp.align(aligned_cloud);
            
            if (icp.hasConverged()) {
                // 获取变换矩阵
                Eigen::Matrix4f transform_matrix = icp.getFinalTransformation();
              
                // 发布 TF
                if(icp.getFitnessScore()<0.001)
                publishTF(transform_matrix, msg->header.stamp);
                //下面这行引起 miss！
                RCLCPP_INFO(this->get_logger(), "Published TF with convergence score: %f", icp.getFitnessScore());
                float x = transform_matrix(0, 3);
                float y = transform_matrix(1, 3);   
                RCLCPP_INFO(this->get_logger(),"x=%6.3f y=%6.3f",x,y); 
               
            } else {
                RCLCPP_WARN(this->get_logger(), "ICP did not converge!");
            }
        }
    }

    void convertLaserToPointCloud(const sensor_msgs::msg::LaserScan::SharedPtr msg,
                                  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud) {
        cloud->clear();
        for (size_t i = 0; i < msg->ranges.size(); ++i) {
            float range = msg->ranges[i];
            if (range > msg->range_min && range < msg->range_max) {
                float angle = msg->angle_min + i * msg->angle_increment;
                pcl::PointXYZ point;
                point.x = range * cos(angle);
                point.y = range * sin(angle);
                point.z = 0.0f;
                cloud->push_back(point);
            }
        }
    }

    void publishTF(const Eigen::Matrix4f& transform_matrix, const rclcpp::Time& timestamp) {
        geometry_msgs::msg::TransformStamped transform;
        transform.header.stamp = timestamp;
        transform.header.frame_id = "map";  // 参考坐标系
        transform.child_frame_id = "laser"; // 激光雷达坐标系

        // 提取平移和旋转
        transform.transform.translation.x = transform_matrix(0, 3);
        transform.transform.translation.y = transform_matrix(1, 3);
        transform.transform.translation.z = transform_matrix(2, 3);

        Eigen::Quaternionf quaternion(transform_matrix.block<3, 3>(0, 0));
        transform.transform.rotation.x = quaternion.x();
        transform.transform.rotation.y = quaternion.y();
        transform.transform.rotation.z = quaternion.z();
        transform.transform.rotation.w = quaternion.w();

        // 发布 TF
        tf_broadcaster_->sendTransform(transform);
    }
    ////去掉车体中心轴点

    rclcpp::Subscription<sensor_msgs::msg::LaserScan>::SharedPtr laser_sub_;
    rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr cloud_sub;
    std::shared_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
    pcl::PointCloud<pcl::PointXYZ>::Ptr reference_cloud_;
    pcl::PointCloud<pcl::PointXYZ>::Ptr init_cloud_;
   // pcl::PointCloud<pcl::PointXYZ> laser_cloud;
    bool first_frame_received_;
    int yaml_run_mode;
    float seat_x0,seat_y0;
    int count;
};

int main(int argc, char** argv) {
    rclcpp::init(argc, argv);
    auto node = std::make_shared<LaserToTF>();
    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}